import pandas as pd
import numpy as np
import re
from io import StringIO

def parse_pixel_coords(pixel_str):
    """解析像素坐标字符串"""
    coords = re.findall(r'P\d+:\s*\(([\d\.\-]+),\s*([\d\.\-]+)\)', pixel_str)
    return np.array([[float(x), float(y)] for x, y in coords], dtype=np.float32)

def parse_coordinates(coord_str):
    """解析坐标数据"""
    if isinstance(coord_str, str):
        # 处理字符串格式的坐标
        coords = coord_str.strip('[]').split(',')
        return np.array([float(x.strip()) for x in coords], dtype=np.float32)
    else:
        # 处理已经是数值的情况
        return np.array(coord_str, dtype=np.float32)

def parse_rotation_matrix(matrix_str):
    """解析旋转矩阵字符串"""
    # 移除所有空格和换行符
    matrix_str = matrix_str.replace(' ', '').replace('\n', '')
    # 提取数字
    numbers = re.findall(r'[-+]?\d*\.\d+', matrix_str)
    # 转换为numpy数组
    matrix = np.array([float(n) for n in numbers]).reshape(3, 3)
    return matrix

def parse_translation_vector(vector_str):
    """解析平移向量字符串"""
    # 移除所有空格和换行符
    vector_str = vector_str.replace(' ', '').replace('\n', '')
    # 提取数字
    numbers = re.findall(r'[-+]?\d*\.\d+', vector_str)
    # 转换为numpy数组
    vector = np.array([float(n) for n in numbers])
    return vector

def read_csv_data(file_path):
    """读取CSV文件数据"""
    # 读取文件内容
    with open(file_path, 'r', encoding='utf-8') as f:
        lines = f.readlines()

    # 初始化数据字典
    data = {
        'target_data': [],
        'camera_params': {},
        'camera_extrinsic': {},
        'total_station_data': []
    }
    
    current_section = None
    data_lines = []

    # 逐行处理文件
    for line in lines:
        line = line.strip()
        if not line:
            continue

        # 检查是否是新的部分
        if line.startswith('==='):
            if '靶标数据' in line:
                current_section = 'target_data'
            elif '全站仪测量数据' in line:
                current_section = 'total_station_data'
            elif '相机参数' in line:
                current_section = 'camera_params'
            elif '相机外参' in line:
                current_section = 'camera_extrinsic'
            continue

        # 根据当前部分处理数据
        if current_section == 'camera_params':
            if ',' in line:
                parts = line.split(',')
                if len(parts) == 2:
                    key = parts[0].strip()
                    try:
                        value = float(parts[1].strip())
                        data['camera_params'][key] = value
                    except ValueError:
                        continue

        elif current_section == 'camera_extrinsic':
            if ',' in line and ('相机位置' in line or '相机欧拉角' in line):
                parts = line.split(',')
                if len(parts) == 2:
                    key = parts[0].strip()
                    try:
                        value = float(parts[1].strip())
                        data['camera_extrinsic'][key] = value
                    except ValueError:
                        continue

        elif current_section == 'target_data':
            data_lines.append(line)

        elif current_section == 'total_station_data':
            if ',' in line:
                parts = line.split(',')
                if len(parts) == 2:
                    data['total_station_data'].append({
                        'name': parts[0].strip(),
                        'coordinates': parse_coordinates(parts[1])
                    })

    # 处理靶标数据
    if data_lines:
        df = pd.read_csv(StringIO('\n'.join(data_lines)))
        data['target_data'] = df

    return data

def get_camera_params(data):
    """获取相机参数"""
    return data['camera_params']

def get_camera_extrinsic(data):
    """获取相机外参"""
    return data['camera_extrinsic']

def get_target_data(data):
    """获取靶标数据"""
    return data['target_data']

def get_total_station_data(data):
    """获取全站仪测量数据"""
    return data['total_station_data']

def get_initial_camera_params(data):
    """获取相机初始参数"""
    params = data['camera_params']
    return np.array([
        params['X方向焦距(像素)'],
        params['Y方向焦距(像素)'],
        params['主点X坐标(像素)'],
        params['主点Y坐标(像素)']
    ])

def get_initial_camera_pose(data):
    """获取相机初始位姿"""
    extrinsic = data['camera_extrinsic']
    return np.array([
        extrinsic['相机欧拉角X(度)'],
        extrinsic['相机欧拉角Y(度)'],
        extrinsic['相机欧拉角Z(度)'],
        extrinsic['相机位置X(mm)'],
        extrinsic['相机位置Y(mm)'],
        extrinsic['相机位置Z(mm)']
    ])

def get_target_points(data):
    """获取靶标点数据"""
    target_data = data['target_data']
    Otw_list = []  # 靶标原点在全站仪坐标系下的坐标
    P3w_list = []  # 靶标三号点在全站仪坐标系下的坐标
    image_points_list = []  # 靶标点像素坐标
    R_tc_list = []  # 靶标坐标系相对于相机坐标系的旋转矩阵
    T_tc_list = []  # 靶标坐标系相对于相机坐标系的平移向量
    
    for _, row in target_data.iterrows():
        # 解析像素坐标
        img_pts = parse_pixel_coords(row['靶标点像素坐标'])
        image_points_list.append(img_pts)
        
        # 解析旋转矩阵和平移向量
        R_tc = parse_rotation_matrix(row['靶标坐标系相对于相机坐标系的R_tc'])
        T_tc = parse_translation_vector(row['靶标坐标系相对于相机坐标系的T_tc'])
        R_tc_list.append(R_tc)
        T_tc_list.append(T_tc)
        
        # 解析靶标原点坐标
        Otw = parse_coordinates(row['靶标原点在全站仪坐标系下的坐标Ot_w'])
        Otw_list.append(Otw)
        
        # 解析靶标三号点坐标
        P3w = parse_coordinates(row['靶标三号点在全站仪坐标系下的坐标P3_w'])
        P3w_list.append(P3w)
    
    return Otw_list, P3w_list, image_points_list, R_tc_list, T_tc_list

def get_object_points():
    """获取靶标点在靶标坐标系下的坐标"""
    # 这里使用固定的靶标点坐标，实际应用中可能需要从配置文件中读取
    return np.array([
        [0,   0,   0],    # P1
        [500, 0,   0],    # P2
        [500, 500, 0],    # P3
        [0,   500, 0],    # P4
        [-250,   250,   -300],  # P5
        [750, 250, -300],  # P6
    ], dtype=np.float32)


def print_data_summary(data):
    """打印数据摘要"""
    # 打印相机参数
    print("\n=== 相机参数 ===")
    for key, value in sorted(data['camera_params'].items()):
        print(f"{key}: {value}")

    # 打印相机外参
    print("\n=== 相机外参 ===")
    for key, value in sorted(data['camera_extrinsic'].items()):
        print(f"{key}: {value}")

    # 打印靶标数据
    print("\n=== 靶标数据 ===")
    print(f"数据条数: {len(data['target_data'])}")
    if len(data['target_data']) > 0:
        print("\n第一条数据示例:")
        print(data['target_data'].iloc[0])

    # 打印全站仪测量数据
    print("\n=== 全站仪测量数据 ===")
    print(f"数据条数: {len(data['total_station_data'])}")
    if len(data['total_station_data']) > 0:
        print("\n第一条数据示例:")
        print(data['total_station_data'][0])
